#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include "opencv2\imgproc\imgproc.hpp"

using namespace cv;
using namespace std;

//Threshold for canny
int thresh = 100;

int main(int argc, const char** argv)
{
	// open the video file for reading
	VideoCapture video("C:/Users/Tyler/Desktop/FinalBugVideos/Original/bugs_final.h264"); 

	// if not success, exit program
	if (!video.isOpened())
	{
		cout << "Cannot open the video file" << endl;
		system("pause"); //wait for a key press
		return -1;
	}

	//////////////////////////////////////////namedWindow("New Video", CV_WINDOW_AUTOSIZE); //create a window called "MyVideo"

	//get the width of frames of the video
	double dWidth = video.get(CV_CAP_PROP_FRAME_WIDTH);
	//get the height of frames of the video
	double dHeight = video.get(CV_CAP_PROP_FRAME_HEIGHT); 

	cout << "Frame Size = " << dWidth << "x" << dHeight << endl;

	Size frameSize(static_cast<int>(dWidth), static_cast<int>(dHeight));

	//set up paths to write new videos to
	VideoWriter newVideo("C:/Users/Tyler/Desktop/FinalBugVideos/Converted/bugs_final.avi", CV_FOURCC('D', 'I', 'V', 'X'), 29, frameSize, true); //initialize the VideoWriter object 
	VideoWriter paths("C:/Users/Tyler/Desktop/FinalBugVideos/Converted/bugs_final_paths.avi", CV_FOURCC('D', 'I', 'V', 'X'), 29, frameSize, true); //initialize the VideoWriter object 
	VideoWriter original("C:/Users/Tyler/Desktop/FinalBugVideos/Converted/bugs_final_original.avi", CV_FOURCC('D', 'I', 'V', 'X'), 29, frameSize, true); //initialize the VideoWriter object 

	//if not initialize the VideoWriter successfully, exit the program
	if (!newVideo.isOpened() | !paths.isOpened() | !original.isOpened())
	{
		cout << "ERROR: Failed to write the video" << endl;
		return -1;
	}

	//Matrix for holding path data
	Mat path;

	while (1)
	{

		Mat frame;
		Mat new_frame = Mat::zeros(frame.size(), frame.type());

		// read a new frame from video
		bool bSuccess = video.read(frame);

		//if not success, break loop
		if (!bSuccess)
		{
			cout << "ERROR: Cannot read a frame from video file" << endl;
			break;
		}

		//Threshold the frame - make frame into a binary image
		threshold(frame, new_frame, 200, 255, THRESH_BINARY);


		Mat canny_output;
		vector<vector<Point> > contours;
		vector<Vec4i> hierarchy;

		//Detect edges using canny
		Canny(new_frame, canny_output, thresh, thresh * 2, 3);
		//Find contours
		findContours(canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));

		//Get the moments
		vector<Moments> mu(contours.size());
		for (int i = 0; i < contours.size(); i++)
		{
			mu[i] = moments(contours[i], false);
		}

		//Get the mass centers:
		vector<Point2f> mc(contours.size());
		for (int i = 0; i < contours.size(); i++)
		{
			mc[i] = Point2f(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
		}

		//Draw contours
		Mat drawing = Mat::zeros(canny_output.size(), CV_8UC3);
		Mat point = Mat::zeros(canny_output.size(), CV_8UC3);
		for (int i = 0; i< contours.size(); i++)
		{
			//Blue color
			Scalar color = Scalar(255, 0, 0);
			
			//Draw the contours in blue
			drawContours(drawing, contours, i, color, 2, 8, hierarchy, 0, Point());
			
			//Draw the breadcrumb trail as the path
			circle(point, mc[i], 2, color, -1, 8, 0);
		}

		//Add the current frame's path to the overall path
		path = point + path;
		
		//Blend the path with the contours and binary image
		Mat blend = new_frame + drawing + path;

		//Write the blended frame to the file
		newVideo.write(blend);

		//Write the paths to the pathways file
		paths.write(path);

		//Write the original frame to the new 'original' file
		original.write(frame);

		//Show the blended video in the display window
		///////////////////////////////////imshow("New Video", blend);

		//wait for 'esc' key press for 30ms. If 'esc' key is pressed, break loop
		if (waitKey(10) == 27)
		{
			cout << "esc key is pressed by user" << endl;
			break;
		}
	}

	return 0;
}